%% 2024/03/03
% Surface Trajectory Tracking using Tube-based NMPC by Liang Hongpeng, 171186623@qq.com

%% Tips:
% 首先运行SDPT3-4.0下面的startup.m,否则提示找不到Solver

%% System description
% x' = f(x,u) + Bw * w

%% Initiallization
clear all % 清空所有变量
close all % 关闭所有图形窗口
clc % 清空命令行

%% 可调参数
dt = 0.6;                                    %控制周期
N = 10;                                     % 预测时域 N * dt

%% Disturbance
Bw = [1,0,0;0,1,0;0,0,3];                   % 扰动输入矩阵

wmax_x = 0.0006;                             % 状态x扰动的最大值0.03m/s 0.006 0.00006
wmax_y = 0.0006;                             % 状态y扰动的最大值0.03m/s 
wmax_theta = 0.0006;                         % 状态theta扰动的最大值0.09rad/s

wmax = [wmax_x; wmax_y; wmax_theta];
wmax_norm = norm(wmax);
W_vertex = [wmax_x , wmax_y , wmax_theta;   % 扰动集合的顶点
            wmax_x , -wmax_y, wmax_theta;
            -wmax_x, wmax_y , wmax_theta; 
            -wmax_x, -wmax_y, wmax_theta;
            wmax_x , wmax_y , -wmax_theta; 
            wmax_x , -wmax_y, -wmax_theta;
            -wmax_x, wmax_y , -wmax_theta; 
            -wmax_x, -wmax_y, -wmax_theta;];
% 在这个特定的代码中，W表示一个三维的矩形，表示在xyz轴上的扰动范围
W = Polyhedron(W_vertex);                   % 构造扰动集合的Polyhedron对象

load('./data/demo0.0006_2.mat'); % w
%% 初始化
% 目标点3d pose A B
A=[0;4;0];
%B=[0;0;5];
%B=[0;0;10];
B = [0;0;30];
%管道半径
R = 2;

last_theta_r=2.1318;
pose=[pi*R;0;2.1318];     %当前2d pose [pi*R;0;pi/2]
pose_sim = pose;        %理想2d pose
%% 路径规划
[xr,yr,vxr,vyr] = PathPlan(A,B,R,dt);
n = length(xr);

vr = sqrt(vxr.^2+vyr.^2);
thetar = atan2(vyr,vxr);

%w = zeros(3, n);                  %存储扰动
%% 位置和参考量

x_rs=zeros(1,n);y_rs=zeros(1,n);theta_rs=zeros(1,n);v_rs=zeros(1,n);w_rs=zeros(1,n);%跟踪路径参考量（用于画图）
xs=zeros(1,n-(N-1));ys=zeros(1,n-(N-1));thetas=zeros(1,n-(N-1));vs=zeros(1,n);ws=zeros(1,n);%机器人的位置和输入（用于画图）
xs(1)=pose(1);ys(1)=pose(2);thetas(1)=pose(3);vs(1)=0;ws(1)=0;

%% 开始循环
%for i=1:n-(N-1)
for i=1:57
    % 读取当前轨迹跟随点
    x_r = xr(i);
    y_r = yr(i);
    theta_r = thetar(i);
    v_r = vr(i);
    
%     if i==1
%         last_theta_r=theta_r;
%     end

    w_r=angle_bound(theta_r-last_theta_r)/dt;last_theta_r=theta_r;
    pose_r = [x_r;y_r;theta_r];
    u_r = [v_r;w_r];

    % 更新参考状态
    j = i+N-1;
    ref = [xr(i:j); yr(i:j); thetar(i:j)];

    % 从扰动集合中随机选择扰动
    %w(:,i) = pick_random_Disturbance(W);   

    % 在开始处记录时间
    iteration_start_time = tic;

    % 应用TubeNMPC方法控制律
    [uc_opt, u_opt] = TubeNMPC_Controller(ref, pose, pose_r, pose_sim, u_r);

    % 输出耗时信息
    iteration_elapsed_time = toc(iteration_start_time);% 计算循环耗时
    fprintf('Iteration %d took %.4f seconds\n', i, iteration_elapsed_time);

    % 将控制量输入对象
    pose_sim = robot_model(pose_sim, u_opt, dt); % 理想系统
    pose = robot_model(pose,uc_opt,dt) + 1*Bw * w(:,i); % 真实带噪声

    %将数据记录
    x_rs(i)=x_r;y_rs(i)=y_r;theta_rs(i)=theta_r; % 参考轨迹记录
    v_rs(i)=v_r;w_rs(i)=w_r;
    xs(i+1)=pose(1);ys(i+1)=pose(2);thetas(i+1)=pose(3); % 实际轨迹记录
    vs(i+1)=uc_opt(1);ws(i+1)=uc_opt(2);

    % times
    %i
end

%% 绘图
grid on;
% 对比图
x = linspace(0,(n-(N-1))*dt,n-(N-1));                                                                          %规定横坐标范围0-n/100
plot(x(1:57),xs(1:57),'b.','linewidth',1);hold on;plot(x(1:57),x_rs(1:57),'r.','linewidth',1);hold off;          %x方向跟踪曲线
xlabel('时间(s)','FontSize',13);
ylabel('u(米)','FontSize',13);
title('u方向跟踪曲线','FontSize',13)
legend('实际值','期望值')
set(gca,'xtick',0:3:57)
figure();
plot(x(1:57),ys(1:57),':b','linewidth',1);hold on;plot(x(1:57),y_rs(1:57),'--r','linewidth',1);hold off;          %y方向跟踪曲线
xlabel('时间(s)','FontSize',13);
ylabel('v(米)','FontSize',13);
title('v方向跟踪曲线','FontSize',13)
legend('实际值','期望值')
set(gca,'xtick',0:3:57)
figure();
plot(x(1:57),thetas(1:57),':b','linewidth',1);hold on;plot(x(1:57),theta_rs(1:57),'--r','linewidth',1);hold off;  %转角跟踪曲线
xlabel('时间(s)','FontSize',13);
ylabel('转角(rad)','FontSize',13);
title('转角跟踪曲线','FontSize',13)
legend('实际值','期望值')
set(gca,'xtick',0:3:57)
figure();
plot(x(1:57),vs(1:57),':b','linewidth',1);hold on;plot(x(1:57),v_rs(1:57),'--r','linewidth',1);hold off;          %速度跟踪曲线
xlabel('时间(s)','FontSize',13);
ylabel('速度(m/s)','FontSize',13);
title('速度跟踪曲线','FontSize',13)
legend('实际值','期望值')
set(gca,'xtick',0:3:57)
figure();
plot(x(1:57),ws(1:57),':b','linewidth',1);hold on;plot(x(1:57),w_rs(1:57),'--r','linewidth',1);hold off;          %角速度跟踪曲线
xlabel('时间(s)','FontSize',13);
ylabel('角速度(rad/s)','FontSize',13);
title('角速度跟踪曲线','FontSize',13)
set(gca,'xtick',0:3:57)
legend('实际值','期望值')

%% 实际3d轨迹绘制
figure();
PathPaint(xr(1:57),yr(1:57),xs(1:57),ys(1:57),R);